#include "core.hpp"
#include <typeinfo>

template <typename T>
T calc(T a, T b, uint8_t op)
{
	switch (op)
	{
	case 1: // mov
		return static_cast<T>(a);
	case 2: // neg
		return static_cast<T>(-a);
	case 3: // cmp
		return static_cast<T>(a - b);
	case 4: // add
		return static_cast<T>(a + b);
	case 5: // sub
		return static_cast<T>(a - b);
	case 6: // mul
		return static_cast<T>(a * b);
	case 7: // div
		if (b == 0)
		{
			return 0;
		}
		return static_cast<T>(a / b);
		break;
	case 8: // mod
		if (b == 0)
		{
			return 0;
		}
		return static_cast<T>((uint64_t)a % (uint64_t)b);
		break;
	case 9: // extu
		if (typeid(T) != typeid(uint64_t))
			return 0;
		break;
	case 10: // exti
		if (typeid(T) != typeid(int64_t))
			return 0;
		break;
	}
}
static uint64_t logic(uint64_t a, uint64_t b, uint8_t op)
{
	switch (op)
	{
	case 1: // not
		return ~a;
	case 2: // test
	case 3: // and
		return a & b;
	case 4: // or
		return a | b;
		break;
	case 5: // xor
		return a ^ b;
		break;
	case 6: // shl
		return a << b;
	case 7: // shr
		return a >> b;
	case 8: // sar
		return uint64_t(static_cast<int64_t>(a) >> b);
	case 9: // rol
		return (a << b) | (a >> (64 - b));
	case 10: // ror
		return (a >> b) | (a << (64 - b));
	}
}

CalcCore::CalcCore(MessageBus *message, CoreBus *cbus, int ID)
{
	bus = message;
	this->ID = ID;
	this->cbus = cbus;
	memset(iregs, 0, sizeof(uint64_t) * 64);
	IR = {0};
}
CalcCore &CalcCore::operator=(const CalcCore &src)
{
	this->bus = src.bus;
	this->cbus = src.cbus;
	this->ID = src.ID;
	memcpy(this->iregs, src.iregs, sizeof(uint64_t) * 64);
	return *this;
}
void CalcCore::exeI()
// I指令集
{
	int64_t temp = iregs[IR.rs1][bus->priv];
	switch (IR.op)
	{
	case 0: // nop
		break;
	case 1: // signed calc
		iregs[IR.rd][bus->priv] = calc<int64_t>(iregs[IR.rs1][bus->priv], iregs[IR.rs2][bus->priv], IR.op2);
		break;
	case 2: // unsigned calc
		iregs[IR.rd][bus->priv] = calc<uint64_t>(iregs[IR.rs1][bus->priv], iregs[IR.rs2][bus->priv], IR.op2);
		break;
	case 3: // logic
		iregs[IR.rd][bus->priv] = logic(iregs[IR.rs1][bus->priv], iregs[IR.rs2][bus->priv], IR.op2);
		break;
	case 8: // immediate signed calc
		iregs[IR.rd][bus->priv] = calc<int64_t>(iregs[IR.rd][bus->priv], IR.imm, IR.op2);
		break;
	case 9: // set
		iregs[IR.rd][bus->priv] = (int64_t)(IR.imm);
		break;
	case 0x0c: // load
		bus->dMAR.lock();
		bus->size = IR.op2 & 0x0f;
		bus->access = R;
		if (IR.op2 & 0x10)
			bus->access |= IO;
		bus->MAR = iregs[IR.rs1][bus->priv] + IR.imm;
		bus->cMAR.unlock();
		bus->cMDR.unlock();
		break;
	case 0x0d: // store
		bus->dMAR.lock();
		bus->size = IR.op2 & 0x0f;
		bus->access = W;
		if (IR.op2 & 0x10)
			bus->access |= IO;
		bus->MAR = iregs[IR.rs1][bus->priv] + IR.imm;
		bus->cMAR.unlock();
		bus->MDR = iregs[IR.rd][bus->priv];
		bus->cMDR.unlock();
		break;
	case 0x0e: // jumps
		switch (IR.op2)
		{
		case 0: // branch
			switch (IR.rd)
			{
			case 0:
				break;
			case 1: // eq
				if (temp == 0)
					goto jump;
				break;
			case 2: // gt
				if (temp > 0)
					goto jump;
				break;
			case 3: // ge
				if (temp >= 0)
					goto jump;
				break;
			case 4: // lt
				if (temp < 0)
					goto jump;
				break;
			case 5: // le
				if (temp <= 0)
					goto jump;
				break;
			case 6: // ne
				if (temp != 0)
					goto jump;
				break;
			case 7:
				goto jump;
				break;
			}
			break;
		case 1: // jmp
		jump:
			bus->sregs[Code][bus->priv] += (int64_t)IR.imm;
			break;
		case 2: // call
			iregs[Stack][bus->priv] -= 8;
			bus->dMAR.lock();
			bus->access = W;
			bus->size = 8;
			bus->MAR = iregs[Stack][bus->priv];
			bus->cMAR.unlock();
			bus->MDR = iregs[RetAddr][bus->priv];
			bus->cMDR.unlock();
			iregs[RetAddr][bus->priv] = bus->sregs[Code][bus->priv];
			bus->sregs[Code][bus->priv] += (int64_t)IR.imm;
			break;
		case 3: // ret
			bus->sregs[Code][bus->priv] = iregs[RetAddr][bus->priv];
			bus->dMAR.lock();
			bus->access = R;
			bus->size = 8;
			bus->MAR = iregs[Stack][bus->priv];
			bus->cMAR.unlock();
			bus->cMDR.unlock();
			iregs[Stack][bus->priv] += 8;
			break;
		}
		break;
	case 0x0f: // kernels
		if ((IR.op2 == 1) ^ bus->priv)
		{ // 权限等级一致（trap指令应在 priv 为假时有效，其余指令在 priv 为真时有效）
			switch (IR.op2)
			{
			case 1: // trap，此处只设置中断标记
				bus->sregs[Status][bus->priv] |= IntTR;
				bus->sregs[Cause][bus->priv] = IR.imm;
				bus->sregs[IT][bus->priv] |= 1 << 3;
				break;
			case 8: // iret
				bus->sregs[Status][bus->priv] ^= Priv;
				break;
			case 9: // kload
				iregs[IR.rd][bus->priv] = bus->sregs[IR.rs1][bus->priv];
				break;
			case 0x0a: // kstore
				bus->sregs[IR.rd][bus->priv] = iregs[IR.rs1][bus->priv];
				break;
			case 0x0b: // flush
				bus->fresh = true;
				break;
			}
		}
		else
		{
			bus->sregs[Status][bus->priv] |= (Error | IntTR);
			bus->sregs[Cause][bus->priv] = 0x82;
			bus->sregs[IT][bus->priv] |= 1 << 8;
		}
		break;
	}
}
void CalcCore::exeF()
{
	switch (IR.op)
	{
	case 0x11: // calcs
		if (IR.type == 8)
			fregs[IR.rd] = calc<float>(fregs[IR.rs1], fregs[IR.rs2], IR.op2);
		else
			dregs[IR.rd] = calc<double>(dregs[IR.rs1], dregs[IR.rs2], IR.op2);
		break;
	case 0x13: // cast to float
		fregs[IR.rd] = IR.type == 4 ? iregs[IR.rs1][bus->priv] : static_cast<float>(dregs[IR.rs1]);
		break;
	case 0x14: // cast to double
		dregs[IR.rd] = IR.type == 4 ? iregs[IR.rs1][bus->priv] : fregs[IR.rs1];
		break;
	case 0x15: // cast to int
		iregs[IR.rd][bus->priv] = static_cast<int64_t>(IR.type == 4 ? fregs[IR.rs1] : dregs[IR.rs1]);
		break;
	case 0x1c: // load
		bus->dMAR.lock();
		bus->access = R;
		bus->size = IR.op2;
		bus->MAR = iregs[IR.rs1][bus->priv] + IR.imm;
		bus->cMAR.unlock();
		bus->cMDR.unlock();
		break;
	case 0x1d: // store
		bus->dMAR.lock();
		bus->access = W;
		bus->size = IR.op2;
		bus->MAR = iregs[IR.rs1][bus->priv] + IR.imm;
		bus->cMAR.unlock();
		bus->MDR = 0;
		if (IR.op2 == 4)
			memcpy(&bus->MDR, fregs + IR.rs2, 4);
		else
			memcpy(&bus->MDR, dregs + IR.rs2, 8);
		bus->cMDR.unlock();
		break;
	}
}
void CalcCore::exeM()
{
	switch (IR.op)
	{
	case 0x21: // signed calc
		iregs[IR.rd][bus->priv] = calc<int64_t>(iregs[IR.rs1][bus->priv], iregs[IR.rs2][bus->priv], IR.op2);
		break;
	case 0x22: // unsigned calc
		iregs[IR.rd][bus->priv] = calc<uint64_t>(iregs[IR.rs1][bus->priv], iregs[IR.rs2][bus->priv], IR.op2);
		break;
	case 0x23: // signed calc,upper 32bit
		iregs[IR.rd][bus->priv] = calc<int64_t>(int64_t(iregs[IR.rs1][bus->priv]) >> 32, int64_t(iregs[IR.rs2][bus->priv]) >> 32, IR.op2);
		break;
	case 0x24: // unsigned calc,upper 32bit
		iregs[IR.rd][bus->priv] = calc<uint64_t>(iregs[IR.rs1][bus->priv] >> 32, iregs[IR.rs2][bus->priv] >> 32, IR.op2);
		break;
	}
}
void CalcCore::exeC()
{
	if (bus->sregs[Status][bus->priv] ^ Master) // 只有主核心能够设置
		return;
	switch (IR.op)
	{
	case 0x30:			  // seti
		if (IR.op2 == ID) // 禁止对自身设置
			break;
		*cbus = std::make_tuple(IR.op2, IR.rd, iregs[IR.rd][bus->priv]);
		break;
	case 0x31:			  // sets
		if (IR.op2 == ID) // 禁止对自身设置
			break;
		*cbus = std::make_tuple(IR.op2, IR.rd | 0x80, bus->sregs[IR.rd][bus->priv]);
		break;
	case 0x33: // mireg
		if (IR.op2 == 0)
			iregs[IR.rd][bus->priv] = iregs[IR.rd][!(bus->priv)];
		else
			iregs[IR.rd][!(bus->priv)] = iregs[IR.rd][bus->priv];
		break;
	case 0x37: // msreg
		if (IR.op2 == 0)
			bus->sregs[IR.rd][bus->priv] = bus->sregs[IR.rd][!(bus->priv)];
		else
			bus->sregs[IR.rd][!(bus->priv)] = bus->sregs[IR.rd][bus->priv];
		break;
	default:
		break;
	}
}
void CalcCore::fetch() // 取指令流程
{
	bus->cMDR.unlock();
	bus->dMAR.lock();
	bus->access = R | X;
	bus->size = 8;
	bus->MAR = bus->sregs[Code][bus->priv] << 3;
	bus->cMAR.unlock();
	bus->sregs[Code][bus->priv] += 1;
	bus->dMDR.lock(); // 等待读操作完成
	bus->access = bus->size = 0;
}
void CalcCore::decode() // 解码指令流程
{
	uint64_t ins = bus->MDR;
	IR.op = ins & 0xff;
	IR.op2 = (ins >> 16) & 0xff;
	if ((IR.op == 0xff) && (IR.op2 == 0xff))
	{
		bus->sregs[Status][bus->priv] |= Halt;
		return;
	}
	IR.rd = (ins >> 8) & 0x1f;
	IR.rs1 = (ins >> 24) & 0x1f;
	IR.rs2 = (ins >> 32) & 0x1f;
	IR.type = (ins >> 40) & 0x1f;
	if ((IR.op & 0x0f) < 4)
		IR.imm = (int64_t)(ins) >> 40;
	else
		IR.imm = (int64_t)(ins) >> 32;
	bus->MDR = 0;
}
void CalcCore::execute() // 执行指令流程
{
	uint8_t isa = (IR.op >> 4) & 0x0f;
	if (!((1 << isa) & ISA))
	{
		bus->sregs[Status][bus->priv] |= (Error | IntTR);
		bus->sregs[Cause][bus->priv] = 0x87;
		bus->sregs[IT][bus->priv] |= 1 << 8;
	}
	switch (isa)
	{
	case 0:
		exeI();
		break;
	case 1:
		exeF();
		break;
	case 2:
		exeM();
		break;
	case 3:
		exeC();
		break;
	}
}
void CalcCore::memory() // 访存流程
{
	if (bus->access == 0) // 无读写，退出
		return;
	bus->dMDR.lock(); // 等待读写操作完成
	switch (IR.op)	  // 只需要处理读操作
	{
	case 0x0c:
		iregs[IR.rd][bus->priv] = bus->MDR;
		break;
	case 0x1c:
		if (IR.op2 == 4)
			memcpy(fregs + IR.rd, &bus->MDR, 4);
		else
			memcpy(dregs + IR.rd, &bus->MDR, 8);
		break;
	case 0x0e:
		if (IR.op2 == 3)
			iregs[RetAddr][bus->priv] = bus->MDR;
		break;
	}
}
void CalcCore::post()
{
	// 首先检查是否是不可屏蔽中断
	if (bus->sregs[IT][bus->priv] & 0x0f) // 不可屏蔽中断
	{
		/* 中断处理流程:
		 * 1.关中断
		 * 2.特权级切换
		 * 3.压栈状态寄存器
		 * 4.读取中断向量*/
	trigged:
		bus->sregs[Status][bus->priv] ^= IntEN; // 关中断
		bus->sregs[Status][bus->priv] |= Priv;	// 切换特权级
		bus->priv = true;						// 从用户态切换到内核态
		// 压栈状态寄存器
		iregs[Stack][bus->priv] -= 8;
		bus->dMAR.lock();
		bus->access = W;
		bus->size = 8;
		bus->MAR = iregs[Stack][bus->priv];
		bus->MDR = bus->sregs[Status][bus->priv];
		bus->cMAR.unlock();
		bus->cMDR.unlock();
		bus->dMDR.lock();
		// 读取中断向量
		bus->dMAR.lock();
		bus->access = R;
		bus->size = 8;
		bus->MAR = bus->sregs[IDTR][bus->priv] << 12;			 // 中断向量表必须从整页开始存放
		bus->MAR += (bus->sregs[Cause][bus->priv] & 0x1ff) << 3; // 中断向量取低九位
		bus->cMAR.unlock();
		bus->cMDR.unlock();
		bus->dMDR.lock();
		bus->sregs[Code][bus->priv] = bus->MDR; // 加载中断向量
		bus->sregs[Status][bus->priv] ^= IntTR; // 清除中断标记
		return;
	}
	else // 可屏蔽中断
	{
		if (bus->sregs[Status][bus->priv] & IntEN)
			goto trigged;
	}
}
void CalcCore::run()
{
	while (bus->sregs[Status][bus->priv] ^ Halt)
	{
		if (bus->sregs[Status][bus->priv] & Slave) // 主核心不接受这个信息
		{
			if (std::get<0>(*cbus) == ID)
			{
				if (std::get<1>(*cbus) == 0xff) // 不传递值
					;
				else if (std::get<1>(*cbus) > 0x80) // 传递sregs值
				{
					bus->sregs[std::get<1>(*cbus) & 0x07][bus->priv] = std::get<2>(*cbus);
					*cbus = {0xff, 0xff, 0};
				}
				else // 传递iregs值
				{
					iregs[std::get<1>(*cbus)][bus->priv] = std::get<2>(*cbus);
					*cbus = {0xff, 0xff, 0};
				}
			}
		}
		if (bus->sregs[Status][bus->priv] & Block) // 被阻塞，无法启动
			continue;
		fetch();
		if (!(bus->sregs[Status][bus->priv] & (Halt | Error)))
			decode();
		if (!(bus->sregs[Status][bus->priv] & (Halt | Error)))
			execute();
		if (!(bus->sregs[Status][bus->priv] & (Halt | Error)))
			memory();
		if (!(bus->sregs[Status][bus->priv] & Halt))
		{
			if (bus->sregs[Status][bus->priv] & IntTR) // 检测到中断标记
				post();
			else
				;
		}
		else
			break;
	}
	bus->size = -1; // 设置关机标志
	bus->cMAR.unlock();
	bus->cMDR.unlock();
}